
#include "SingleSweepAndPrune.h"
#include "libmath.h"

namespace p2 {
    
SingleSweepAndPrune::~SingleSweepAndPrune()
{
}

void SingleSweepAndPrune::updateNotify()
{
    SapPairMap::iterator iter;
    for (iter = _pairManager.active().begin();
         iter != _pairManager.active().end(); ++iter) {
        SapPairMapElement const& pair = (*iter);
        _observer->removePair(_objects[pair.second._id0]._data,
                              _objects[pair.second._id1]._data);
    }
    for (iter = _pairManager.active().begin();
         iter != _pairManager.active().end(); ++iter) {
        SapPairMapElement const& pair = (*iter);
        _observer->addPair(_objects[pair.second._id0]._data,
                           _objects[pair.second._id1]._data);
    }
}

bool SingleSweepAndPrune::endPointBatchSortPredicate(SapEndPoint const& ep0, SapEndPoint const& ep1)
{
    return ep0._val > ep1._val;
}

#ifdef DEBUG
void SingleSweepAndPrune::dumpEndPoints()
{
    for (uint32 a = 0; a < 2; a++) {
        for (uint32 i = 1; i < _endPoints[a].size() - 1; i++) {
            SapEndPoint& endPoint = _endPoints[a][i];
            LOGF("endPoint._val=%f\n", endPoint._val);
            uint32 boxId = endPoint.boxId();
            if (endPoint.isMin()) {
            	LOGF("obj[%ld].min[%ld]=%f\n",
                       boxId, a, _endPoints[a][ _objects[boxId]._iMin[a] ]._val);
            } else {
            	LOGF("obj[%ld].max[%ld]=%f\n",
                       boxId, a, _endPoints[a][ _objects[boxId]._iMax[a] ]._val);
            }
        }
    }
    fflush(stdout);
}

void SingleSweepAndPrune::drawEndPoints()
{
}

void SingleSweepAndPrune::logMetrics()
{
	LOGF("SingleSweepAndPrune: %ld object(s):\n", _objects.size());
	LOGF("\t(%ld, %ld) axis endpoints\n",
           _endPoints[0].size(), _endPoints[1].size());
    // min->max ordering for both axes
	LOGF("\t(%ld, %ld) axis batch endpoints\n",
           _axisBatch[0].size(), _axisBatch[1].size());
    // batch for each axis
	LOGF("\t%ld active pairs\n", _pairManager.active().size());
}
#endif	//DEBUG
void SingleSweepAndPrune::commitBatchInsert()
{
    // insert batches in reverse
    for (uint32 axisIndex = 0; axisIndex < 2; axisIndex++) {
        // sort batches max->min
        std::sort(_axisBatch[axisIndex].begin(), _axisBatch[axisIndex].end(), endPointBatchSortPredicate);
        
        std::vector<SapEndPoint>::reverse_iterator riter = _endPoints[axisIndex].rbegin();
        uint32 sizeAB = _axisBatch[axisIndex].size();
        for (uint32 epIndex = 0; epIndex < sizeAB;) {
            SapEndPoint& endPoint = _axisBatch[axisIndex][epIndex];
            ASSERTLOG(endPoint._val > -Math<real>::MAX_REAL && endPoint._val < +Math<real>::MAX_REAL, "SingleSweepAndPrune::commitBatchInsert");
            if (endPoint._val > (*riter)._val) {
                // insert new end point here to maintain order
                riter = std::vector<SapEndPoint>::reverse_iterator(_endPoints[axisIndex].insert(riter.base(), endPoint));
                ++epIndex;
            } else {
                ++riter;
            }
        }
        // set the object indices
        uint32 startIndex = riter.base() - _endPoints[axisIndex].begin();
        for (uint32 epIndex = startIndex;
             epIndex < _endPoints[axisIndex].size() - 1; epIndex++) {
            SapEndPoint& endPoint = _endPoints[axisIndex][epIndex];
            if (endPoint.isMin()) {
                _objects[endPoint.boxId()]._iMin[axisIndex] = epIndex;
            }
            else {
                _objects[endPoint.boxId()]._iMax[axisIndex] = epIndex;
            }
        }
        
        // do not deallocate any memory
        _axisBatch[axisIndex].resize(0);
    }
    // second pass on this axis to find overlapping pairs
    for (uint32 axisIndex = 0; axisIndex < 2; axisIndex++) {
        uint32 sizeO = _objects.size();
        for (; _batchStartObjectId < sizeO; ++_batchStartObjectId) {
            SapObject& obj0 = _objects[_batchStartObjectId];
            
            ASSERTLOG(_endPoints[axisIndex][obj0._iMin[axisIndex]].boxId()==_batchStartObjectId, "SingleSweepAndPrune::commitBatchInsert");
            ASSERTLOG(_endPoints[axisIndex][obj0._iMax[axisIndex]].boxId()==_batchStartObjectId, "SingleSweepAndPrune::commitBatchInsert");

            // => find each object between this object's endpoints
            // and add it and this object as a pair
            for (int32 ti = obj0._iMin[axisIndex] + 1;
                 ti < obj0._iMax[axisIndex]; ti++) {
                SapEndPoint& endPoint = _endPoints[axisIndex][ti];
                if (overlap(_batchStartObjectId, endPoint.boxId())) {
                    _pairManager.add((uint16) _batchStartObjectId,
                                     (uint16) endPoint.boxId());
                }
            }
        }
    }
}

void SingleSweepAndPrune::commitBatchDelete()
{
    for (int axis = 0; axis < 2; axis++) {
        std::vector<SapEndPoint>::iterator iter = _endPoints[axis].begin();
        while (iter != _endPoints[axis].end()) {
            if ((*iter).isMarkedForDeletion()) {
                iter = _endPoints[axis].erase(iter);
            } else {
                ++iter;
            }
        }
    }
}

uint32 SingleSweepAndPrune::addObject(Aabb2<real> const& box, uint32 plane, void* userData, bool commitObject /*= true*/)
{
    uint32 boxId = _objects.size();
    
    // add end points to batch
    SapEndPoint ep;
    ep.setBoxId(boxId);
    ep.setIsMin(true);
    ep._val = box.min().X();
    _axisBatch[0].push_back(ep);
    ep.setBoxId(boxId);
    ep.setIsMin(true);
    ep._val = box.min().Y();
    _axisBatch[1].push_back(ep);
    ep.setBoxId(boxId);
    ep.setIsMin(false);
    ep._val = box.max().X();
    _axisBatch[0].push_back(ep);
    ep.setBoxId(boxId);
    ep.setIsMin(false);
    ep._val = box.max().Y();
    _axisBatch[1].push_back(ep);
    
    SapObject obj;
    obj._plane = plane;
    obj._data = userData;
    _objects.push_back(obj);
    
    if (commitObject) {
        commitBatchInsert();
    }
    return boxId;
}

void SingleSweepAndPrune::checkEndPointBubbleUp(uint32 endPointIndex,
                                                uint32 axis)
{
    uint32 testIndex = endPointIndex;
    while (_endPoints[axis][testIndex]._val
           > _endPoints[axis][testIndex + 1]._val) {
        if (_endPoints[axis][testIndex].boxId()
            != _endPoints[axis][testIndex + 1].boxId()) {
            if (!_endPoints[axis][testIndex].isMin()
                && _endPoints[axis][testIndex + 1].isMin()) {
                if (overlap(_endPoints[axis][testIndex].boxId(),
                            _endPoints[axis][testIndex + 1].boxId())) {
                    // this is max next is min => add pair
                    uint16 id0 = (uint16) _endPoints[axis][testIndex].boxId();
                    uint16 id1 =
                    (uint16) _endPoints[axis][testIndex + 1].boxId();
                    _pairManager.add(id0, id1);
                }
            } else if (_endPoints[axis][testIndex].isMin()
                       && !_endPoints[axis][testIndex + 1].isMin()) {
                // this is min next is max => remove pair
                uint16 id0 = (uint16) _endPoints[axis][testIndex].boxId();
                uint16 id1 = (uint16) _endPoints[axis][testIndex + 1].boxId();
                _pairManager.remove(id0, id1);
            }
        }
        swapEndPoints(testIndex, testIndex + 1, axis);
        ++testIndex;
    }
}

void SingleSweepAndPrune::checkEndPointBubbleDown(uint32 endPointIndex,
                                                  uint32 axis)
{
    uint32 testIndex = endPointIndex;
    while (_endPoints[axis][testIndex]._val
           < _endPoints[axis][testIndex - 1]._val) {
        if (_endPoints[axis][testIndex].boxId()
            != _endPoints[axis][testIndex - 1].boxId()) {
            if (_endPoints[axis][testIndex].isMin()
                && !_endPoints[axis][testIndex - 1].isMin()) {
                if (overlap(_endPoints[axis][testIndex].boxId(),
                            _endPoints[axis][testIndex - 1].boxId())) {
                    // this is min last is max => add pair
                    uint16 id0 = (uint16) _endPoints[axis][testIndex].boxId();
                    uint16 id1 =
                    (uint16) _endPoints[axis][testIndex - 1].boxId();
                    _pairManager.add(id0, id1);
                }
            } else if (!_endPoints[axis][testIndex].isMin()
                       && _endPoints[axis][testIndex - 1].isMin()) {
                // this is max last is min => remove pair
                uint16 id0 = (uint16) _endPoints[axis][testIndex].boxId();
                uint16 id1 = (uint16) _endPoints[axis][testIndex - 1].boxId();
                _pairManager.remove(id0, id1);
            }
        }
        swapEndPoints(testIndex, testIndex - 1, axis);
        --testIndex;
    }
}

void SingleSweepAndPrune::updateObject(Aabb2<real> const& box, uint32 objId)
{
    SapObject& obj = _objects[objId];
    _endPoints[0][obj._iMin[0]]._val = box.min().X();
    _endPoints[1][obj._iMin[1]]._val = box.min().Y();
    _endPoints[0][obj._iMax[0]]._val = box.max().X();
    _endPoints[1][obj._iMax[1]]._val = box.max().Y();
    for (uint32 axis = 0; axis < 2; axis++) {
        checkEndPointBubbleUp(obj._iMin[axis], axis);
        checkEndPointBubbleDown(obj._iMin[axis], axis);
        checkEndPointBubbleUp(obj._iMax[axis], axis);
        checkEndPointBubbleDown(obj._iMax[axis], axis);
    }
}

void SingleSweepAndPrune::removeObject(uint32 /*objId*/)
{
}

}
